import numpy as np
import math
a = np.asarray([1.0,1.0,1.0])
theta = math.radians(45)
# print(theta)
R_AB = np.asarray([math.cos(theta),-math.sin(theta),0,
                      math.sin(theta),math.cos(theta),0,
                      0,0,1]).reshape(3,3)
# print(R_AB)

'''compute R_BP and P_BP'''
# Coordinate System CS

# the position of p to c using CS:C
P_CP = np.asarray([2,1,2]).reshape(3,1)

# the position of p to b using CS:B
P_BC = np.asarray([0,0,3]).reshape(3,1)

theta = math.radians(180)
R_BC = np.asarray([1,0,0,
                     0,math.cos(theta),-math.sin(theta),
                     0,math.sin(theta),math.cos(theta)]).reshape(3,3)
R_BP = R_BC*np.identity(3)

temp  = np.dot(R_BC,P_CP)
P_BP = np.add(temp,P_BC)

print(R_BP)
print(P_BP.T)




'''qi ci  bian huan jv zhen'''
import transforms3d as tf3

# use np
R = np.asarray([1,0,0,
                0,1,0,
                0,0,1
                ]).reshape(3,3)
T = np.asarray([1,0,1]).reshape(3,1)
temp = np.hstack((R,T))
M = np.vstack((temp,[0,0,0,1]))
print(M)

# use tf3
R = np.asarray([1,0,0,
                0,1,0,
                0,0,1
                ]).reshape(3,3)
T = np.asarray([1,0,1])
M = tf3.affines.compose(T,R,[1,1,1])
print(M)

# example 1.3.1
# object: O, Camera: C Base: B

P_CO = np.asarray([2,1,2])
R_CO = np.identity(3)

temp = np.hstack((R_CO,P_CO.reshape(3,1)))
T_CO = np.vstack((temp,[0,0,0,1]))

T_CO = tf3.affines.compose(P_CO,R_CO,[1,1,1])
# example 1.3.2
np.set_printoptions(suppress=True)
P_BC = np.asarray([0,0,3])
theta = math.radians(180)
R_BC = np.asarray([1,0,0,
                   0,math.cos(theta),-math.sin(theta),
                   0,math.sin(theta),math.cos(theta)]).reshape(3,3)
T_BC = tf3.affines.compose(P_BC,R_BC,[1,1,1])
print(T_CO)
print(T_BC)

# example 3.1
T_BO = np.dot(T_BC,T_CO)
P_BO = T_BO[:3,3:4]
R_BO = T_BO[:3,:3]
print(T_BO)
print(P_BO)
print(R_BO)
